trusty: Add fpu/simd support

The original patch has been partly merged. This adds the missing
pieces.

Change-Id: I77fd434feab396ff05d9b8e0c1761e4dd588a701
Signed-off-by: Arve Hjønnevåg <[email protected]>

diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index 08767bb53f3998906c8d27a369e09b9bec5fdc6b..6e80b7b7238e785ae782f8b56223bf50c99752c8 100644 (file)
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -105,10 +105,8 @@ static struct args trusty_context_switch(uint32_t security_state, uint64_t r0,
         * when it's needed the PSCI caller has preserved FP context before
         * going here.
         */
-#if CTX_INCLUDE_FPREGS
        if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
                fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state)));
-#endif
        cm_el1_sysregs_context_save(security_state);
 
        ctx->saved_security_state = security_state;
@@ -117,10 +115,8 @@ static struct args trusty_context_switch(uint32_t security_state, uint64_t r0,
        assert(ctx->saved_security_state == !security_state);
 
        cm_el1_sysregs_context_restore(security_state);
-#if CTX_INCLUDE_FPREGS
        if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
                fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state)));
-#endif
 
        cm_set_next_eret_context(security_state);
 
@@ -299,6 +295,7 @@ static int32_t trusty_init(void)
        ep_info = bl31_plat_get_next_image_ep_info(SECURE);
        assert(ep_info);
 
+       fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
        cm_el1_sysregs_context_save(NON_SECURE);
 
        cm_set_context(&ctx->cpu_ctx, SECURE);
@@ -315,6 +312,7 @@ static int32_t trusty_init(void)
        }
 
        cm_el1_sysregs_context_restore(SECURE);
+       fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
        cm_set_next_eret_context(SECURE);
 
        ctx->saved_security_state = ~0; /* initial saved state is invalid */
@@ -323,6 +321,7 @@ static int32_t trusty_init(void)
        trusty_context_switch_helper(&ctx->saved_sp, &zero_args);
 
        cm_el1_sysregs_context_restore(NON_SECURE);
+       fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
        cm_set_next_eret_context(NON_SECURE);
 
        return 0;

diff --git a/services/spd/trusty/trusty.mk b/services/spd/trusty/trusty.mk
index 2e51bd5cbc123f731d8c4e78356bb2d08753b5cf..a571fa2cdb5ee959e3a73d87d2818bb39a4f2f81 100644 (file)
--- a/services/spd/trusty/trusty.mk
+++ b/services/spd/trusty/trusty.mk
@@ -14,3 +14,5 @@ SPD_SOURCES           +=      services/spd/trusty/generic-arm64-smcall.c
 endif
 
 NEED_BL32              :=      yes
+
+CTX_INCLUDE_FPREGS     :=      1